#include "mapOptmization.hpp"

#include <iostream>
#include <pcl/filters/crop_box.h>
#include <pcl/kdtree/kdtree_flann.h>

// #include <std_msgs/Float64MultiArray.h>
// #include <std_msgs/Header.h>
// #include <visualization_msgs/Marker.h>
// #include <visualization_msgs/MarkerArray.h>

// gstam
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>

ros::Publisher pubLaserCloudSurround;
ros::Publisher pubOptimizedGlobalMap;  //   发布最后优化的地图

// voxel filter paprams
float odometrySurfLeafSize;
float mappingCornerLeafSize;
float mappingSurfLeafSize;

// CPU Params
int numberOfCores = 4;

nav_msgs::Path globalPath;

// Surrounding map
float surroundingkeyframeAddingDistThreshold;   //  判断是否为关键帧的距离阈值
float surroundingkeyframeAddingAngleThreshold;  //  判断是否为关键帧的角度阈值

bool recontructKdTree = false;
int updateKdtreeCount = 0;        
bool visulize_IkdtreeMap = false;  //  visual iktree submap

// global map visualization radius
float globalMapVisualizationSearchRadius;
float globalMapVisualizationPoseDensity;
float globalMapVisualizationLeafSize;

gtsam::Values initialEstimate;
gtsam::Values optimizedEstimate;
gtsam::ISAM2* isam;
gtsam::Values isamCurrentEstimate;
Eigen::MatrixXd poseCovariance;

// saveMap
bool savePCD;             // 是否保存地图
string savePCDDirectory;  // 保存路径

// 更新里程计轨迹
void updatePath(const PointTypePose& pose_in) {
    string odometryFrame = "camera_init";
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);

    pose_stamped.header.frame_id = odometryFrame;
    pose_stamped.pose.position.x = pose_in.x;
    pose_stamped.pose.position.y = pose_in.y;
    pose_stamped.pose.position.z = pose_in.z;
    tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
    pose_stamped.pose.orientation.x = q.x();
    pose_stamped.pose.orientation.y = q.y();
    pose_stamped.pose.orientation.z = q.z();
    pose_stamped.pose.orientation.w = q.w();

    globalPath.poses.push_back(pose_stamped);
}

// 对点云cloudIn进行变换transformIn，返回结果点云， 修改liosam, 考虑到外参的表示
CloudPtr transformPointCloud(CloudPtr cloudIn, PointTypePose* transformIn) {
    CloudPtr cloudOut(new CloudType());

    int cloudSize = cloudIn->size();
    cloudOut->resize(cloudSize);

    // 注意：lio_sam 中的姿态用的euler表示，而fastlio存的姿态角是旋转矢量。而 pcl::getTransformation是将euler_angle 转换到rotation_matrix 不合适，注释
    // Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch,
    // transformIn->yaw);
    Eigen::Isometry3d T_b_lidar(state_out.offset_R_L_I);  //  获取  body2lidar  外参
    T_b_lidar.pretranslate(state_out.offset_T_L_I);

    Eigen::Affine3f T_w_b_ = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
    Eigen::Isometry3d T_w_b;  //   world2body
    T_w_b.matrix() = T_w_b_.matrix().cast<double>();

    Eigen::Isometry3d T_w_lidar = T_w_b * T_b_lidar;  //  T_w_lidar  转换矩阵

    Eigen::Isometry3d transCur = T_w_lidar;

#pragma omp parallel for num_threads(numberOfCores)
    for (int i = 0; i < cloudSize; ++i) {
        const auto& pointFrom = cloudIn->points[i];
        cloudOut->points[i].x = transCur(0, 0) * pointFrom.x + transCur(0, 1) * pointFrom.y + transCur(0, 2) * pointFrom.z + transCur(0, 3);
        cloudOut->points[i].y = transCur(1, 0) * pointFrom.x + transCur(1, 1) * pointFrom.y + transCur(1, 2) * pointFrom.z + transCur(1, 3);
        cloudOut->points[i].z = transCur(2, 0) * pointFrom.x + transCur(2, 1) * pointFrom.y + transCur(2, 2) * pointFrom.z + transCur(2, 3);
        cloudOut->points[i].intensity = pointFrom.intensity;
    }
    return cloudOut;
}

// 将更新的pose赋值到 transformTobeMapped
void getCurPose(state_output cur_state) {
    //  欧拉角是没有群的性质，所以从SO3还是一般的rotation matrix 转换过来的结果一样
    Eigen::Vector3d eulerAngle = cur_state.rot.matrix().eulerAngles(2, 1, 0);  //  yaw pitch roll  单位：弧度
    // V3D eulerAngle  =  SO3ToEuler(cur_state.rot)/57.3 ;     //   fastlio 自带  roll pitch yaw  单位: 度，旋转顺序 zyx

    // eulerAngle(0);                //  roll     使用 SO3ToEuler 方法时，顺序是 rpy

    transformTobeMapped[0] = eulerAngle(2);     //  roll  使用 eulerAngles(2,1,0) 方法时，顺序是 ypr
    transformTobeMapped[1] = eulerAngle(1);     //  pitch
    transformTobeMapped[2] = eulerAngle(0);     //  yaw
    transformTobeMapped[3] = cur_state.pos(0);  //  x
    transformTobeMapped[4] = cur_state.pos(1);  //   y
    transformTobeMapped[5] = cur_state.pos(2);  // z
}

void publish_path_update(const ros::Publisher pubPath) {
    ros::Time timeLaserInfoStamp = ros::Time().fromSec(lidar_end_time);  //  时间戳
    string odometryFrame = "camera_init";
    if (pubPath.getNumSubscribers() != 0) {
        /*** if path is too large, the rvis will crash ***/
        static int kkk = 0;
        kkk++;
        if (kkk % 10 == 0) {
            // path.poses.push_back(globalPath);
            globalPath.header.stamp = timeLaserInfoStamp;
            globalPath.header.frame_id = odometryFrame;
            pubPath.publish(globalPath);
        }
    }
}

#include <Eigen/Geometry>

void saveTrajectoryTUM(const CloudPtrPose& KeyPoses, const std::string& fileName) {
    // Save optimized data in TUM format
    std::ofstream file_pose_tum(std::string(ROOT_DIR) + "Log/" + fileName);
    for (int i = 0; i < KeyPoses->size(); i++) {
        Eigen::Vector3d t(KeyPoses->points[i].x, KeyPoses->points[i].y, KeyPoses->points[i].z);
        Eigen::Matrix3d R = Exp(double(KeyPoses->points[i].roll), double(KeyPoses->points[i].pitch), double(KeyPoses->points[i].yaw));

        // Convert rotation matrix to quaternion
        Eigen::Quaterniond q(R);

        file_pose_tum << std::fixed << std::setprecision(9) << KeyPoses->points[i].time << " " << std::setprecision(5) << t.x() << " " << t.y() << " " << t.z()
                      << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
    }
    file_pose_tum.close();
}

// 发布局部关键帧map的特征点云
void publishGlobalMap() {
    /*** if path is too large, the rvis will crash ***/
    ros::Time timeLaserInfoStamp = ros::Time().fromSec(lidar_end_time);
    string odometryFrame = "camera_init";
    if (pubLaserCloudSurround.getNumSubscribers() == 0) return;

    if (cloudKeyPoses3D->points.empty() == true) return;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());
    ;
    CloudPtr globalMapKeyPoses(new CloudType());
    CloudPtr globalMapKeyPosesDS(new CloudType());
    CloudPtr globalMapKeyFrames(new CloudType());
    CloudPtr globalMapKeyFramesDS(new CloudType());

    // kdtree查找最近一帧关键帧相邻的关键帧集合
    std::vector<int> pointSearchIndGlobalMap;
    std::vector<float> pointSearchSqDisGlobalMap;
    mtx.lock();
    kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
    kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
    mtx.unlock();

    for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i) globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
    // 降采样
    pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses;
    downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity,
                                                globalMapVisualizationPoseDensity);  // for global map visualization
    downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
    downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
    // 提取局部相邻关键帧对应的特征点云
    for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i) {
        // 距离过大
        if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius) continue;
        int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
        *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);  //  fast_lio only use  surfCloud
    }
    // 降采样，发布
    pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames;  // for global map visualization
    downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize,
                                                 globalMapVisualizationLeafSize);  // for global map visualization
    downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
    downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
    publishCloud(&pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);
}

// 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
bool saveFrame() {
    if (cloudKeyPoses3D->points.empty()) return true;
    // 前一帧位姿
    Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
    // 当前帧位姿
    Eigen::Affine3f transFinal = trans2Affine3f(transformTobeMapped);
    // 位姿变换增量
    Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);  //  获取上一帧 相对 当前帧的 位姿

    // 旋转和平移量都较小，当前帧不设为关键帧
    if (abs(roll) < surroundingkeyframeAddingAngleThreshold && abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
        abs(yaw) < surroundingkeyframeAddingAngleThreshold && sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
        return false;
    return true;
}

// 添加激光里程计因子
void addOdomFactor() {
    // 第一帧初始化先验因子
    if (cloudKeyPoses3D->points.empty()) {
        // 创建一个先验噪声模型 priorNoise，表示初始位姿的不确定性。这里使用的是对角协方差矩阵，其中对角线上的值表示各个维度的方差
        // indoor 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12    //  1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8
        gtsam::noiseModel::Diagonal::shared_ptr priorNoise =
            gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12).finished());  // rad*rad, meter*meter
        gtSAMgraph.add(gtsam::PriorFactor<gtsam::Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
        // 变量节点设置初始值
        initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
    } else {
        // 添加激光里程计因子
        gtsam::noiseModel::Diagonal::shared_ptr odometryNoise =
            gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
        gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());  /// pre
        gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);                    // cur
        // 参数：前一帧id，当前帧id，前一帧与当前帧的位姿变换（作为观测值），噪声协方差

        // BetweenFactor类型，表示两个位姿之间的相对关系
        // 第一个参数，表示因子图中前一帧（previous frame）的索引. 第二个参数，表示当前帧（current frame）的索引
        // poseFrom.between(poseTo)：表示前一帧（poseFrom）到当前帧（poseTo）的相对位姿变换。between()函数计算两个位姿之间的相对变换。
        // 第四个参数，表示与激光里程计因子相关的噪声模型。这通常是一个对角矩阵，描述了因子中的不确定性。
        gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
        // 变量节点设置初始值
        initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
    }
}

void saveKeyFramesAndFactor() {
    //  计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
    if (saveFrame() == false) return;

    // ROS_INFO_STREAM("\033[1;32m saveKeyFramesAndFactor: "  << "\033[0m");

    // 激光里程计因子(from fast-lio),  输入的是frame_relative pose  帧间位姿(body 系下)
    addOdomFactor();
    // 闭环因子 (rs-loop-detect)  基于欧氏距离的检测
    addLoopFactor();
    // 执行优化
    isam->update(gtSAMgraph, initialEstimate);
    isam->update();
    if (aLoopIsClosed == true)  // 有回环因子，多update几次
    {
        isam->update();
        isam->update();
        isam->update();
        isam->update();
        isam->update();
    }
    // update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了
    gtSAMgraph.resize(0);
    initialEstimate.clear();

    PointType thisPose3D;
    PointTypePose thisPose6D;
    gtsam::Pose3 latestEstimate;

    // 优化结果
    isamCurrentEstimate = isam->calculateBestEstimate();
    // 当前帧位姿结果
    latestEstimate = isamCurrentEstimate.at<gtsam::Pose3>(isamCurrentEstimate.size() - 1);

    // cloudKeyPoses3D加入当前帧位置
    thisPose3D.x = latestEstimate.translation().x();
    thisPose3D.y = latestEstimate.translation().y();
    thisPose3D.z = latestEstimate.translation().z();
    // 索引
    thisPose3D.intensity = cloudKeyPoses3D->size();  //  使用intensity作为该帧点云的index
    cloudKeyPoses3D->push_back(thisPose3D);          //  新关键帧帧放入队列中

    // cloudKeyPoses6D加入当前帧位姿
    thisPose6D.x = thisPose3D.x;
    thisPose6D.y = thisPose3D.y;
    thisPose6D.z = thisPose3D.z;
    thisPose6D.intensity = thisPose3D.intensity;
    thisPose6D.roll = latestEstimate.rotation().roll();
    thisPose6D.pitch = latestEstimate.rotation().pitch();
    thisPose6D.yaw = latestEstimate.rotation().yaw();
    thisPose6D.time = lidar_end_time;
    cloudKeyPoses6D->push_back(thisPose6D);

    // 位姿协方差
    poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);

    // ESKF状态和方差  更新
    state_output state_updated = kf_output.get_x();  //  获取cur_pose (还没修正)
    Eigen::Vector3d pos(latestEstimate.translation().x(), latestEstimate.translation().y(), latestEstimate.translation().z());
    Eigen::Quaterniond q = EulerToQuat(latestEstimate.rotation().roll(), latestEstimate.rotation().pitch(), latestEstimate.rotation().yaw());

    //  更新状态量
    state_updated.pos = pos;
    state_updated.rot = q;
    state_out = state_updated;  // 对state_point进行更新，state_point可视化用到

    kf_output.change_x(state_updated);  //  对cur_pose 进行isam2优化后的修正

    // TODO:  P的修正有待考察，按照yanliangwang的做法，修改了p，会跑飞
    // esekfom::esekf<state_ikfom, 12, input_ikfom>::cov P_updated = kf.get_P(); // 获取当前的状态估计的协方差矩阵
    // auto P_updated = kf_output.get_P(); // 获取当前的状态估计的协方差矩阵
    // P_updated.setIdentity();
    // P_updated(6, 6) = P_updated(7, 7) = P_updated(8, 8) = 0.00001;
    // P_updated(9, 9) = P_updated(10, 10) = P_updated(11, 11) = 0.00001;
    // P_updated(15, 15) = P_updated(16, 16) = P_updated(17, 17) = 0.0001;
    // P_updated(18, 18) = P_updated(19, 19) = P_updated(20, 20) = 0.001;
    // P_updated(21, 21) = P_updated(22, 22) = 0.00001;
    // kf_output.change_P(P_updated);

    // 当前帧激光角点、平面点，降采样集合
    CloudPtr thisSurfKeyFrame(new CloudType());
    pcl::copyPointCloud(*feats_undistort, *thisSurfKeyFrame);  // 存储关键帧,没有降采样的点云
    
    scManager.makeAndSaveScancontextAndKeys(*thisSurfKeyFrame);
    // 保存特征点降采样集合
    surfCloudKeyFrames.push_back(thisSurfKeyFrame);

    updatePath(thisPose6D);  //  可视化update后的path
}

void recontructIKdTree() {
    updateKdtreeCount++;
    if (recontructKdTree && updateKdtreeCount > 0) {
        // if path is too large, the rvis will crash
        pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMapPoses(new pcl::KdTreeFLANN<PointType>());
        CloudPtr MapKeyPoses(new CloudType());
        CloudPtr MapKeyPosesDS(new CloudType());
        CloudPtr MapKeyFrames(new CloudType());
        CloudPtr MapKeyFramesDS(new CloudType());

        // kdtree查找最近一帧关键帧相邻的关键帧集合
        std::vector<int> pointSearchIndGlobalMap;
        std::vector<float> pointSearchSqDisGlobalMap;
        mtx.lock();
        kdtreeGlobalMapPoses->setInputCloud(cloudKeyPoses3D);
        kdtreeGlobalMapPoses->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
        mtx.unlock();

        for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
            MapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);  //  subMap的pose集合
        // 降采样
        pcl::VoxelGrid<PointType> downSizeFilterSubMapKeyPoses;
        downSizeFilterSubMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity,
                                                 globalMapVisualizationPoseDensity);  // for global map visualization
        downSizeFilterSubMapKeyPoses.setInputCloud(MapKeyPoses);
        downSizeFilterSubMapKeyPoses.filter(*MapKeyPosesDS);  //  subMap poses  downsample
        // 提取局部相邻关键帧对应的特征点云
        for (int i = 0; i < (int)MapKeyPosesDS->size(); ++i) {
            // 距离过大
            if (pointDistance(MapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius) continue;
            int thisKeyInd = (int)MapKeyPosesDS->points[i].intensity;
            *MapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);  //  fast_lio only use  surfCloud
        }
        // 降采样，发布
        pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames;  // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize,
                                                     globalMapVisualizationLeafSize);  // for global map visualization
        downSizeFilterGlobalMapKeyFrames.setInputCloud(MapKeyFrames);
        downSizeFilterGlobalMapKeyFrames.filter(*MapKeyFramesDS);

        std::cout << "MapKeyFramesDS sizes  =  " << MapKeyFramesDS->points.size() << std::endl;

        ikdtree.Build(MapKeyFramesDS->points);
        updateKdtreeCount = 0;
        ROS_INFO_STREAM("\033[1;32m Reconstructed  ikdtree  \033[0m");

        std::cout << "featsFromMapNum  =  " << ikdtree.validnum() << "\t"
                  << " kdtree_size_st   =  " << ikdtree.size() << std::endl;
    }
}

// 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
void correctPoses() {
    if (cloudKeyPoses3D->points.empty()) return;

    if (aLoopIsClosed == true) {
        // 清空里程计轨迹
        globalPath.poses.clear();
        // 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿
        int numPoses = isamCurrentEstimate.size();
        for (int i = 0; i < numPoses; ++i) {
            cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().x();
            cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().y();
            cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<gtsam::Pose3>(i).translation().z();

            cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
            cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
            cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
            cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().roll();
            cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().pitch();
            cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<gtsam::Pose3>(i).rotation().yaw();

            // 更新里程计轨迹
            updatePath(cloudKeyPoses6D->points[i]);
        }
        // 清空局部map， reconstruct  ikdtree submap
        // TODO: 重构ikd反而会炸
        // recontructIKdTree();
        // ROS_INFO("ISMA2 Update");
        aLoopIsClosed = false;
    }
}

void LoadParams(ros::NodeHandle& nh) {
    nh.param<float>("odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
    nh.param<float>("mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
    nh.param<float>("mappingSurfLeafSize", mappingSurfLeafSize, 0.2);

    nh.param<int>("numberOfCores", numberOfCores, 8);
    // nh.param<double>("mappingProcessInterval", mappingProcessInterval, 0.15);

    // save keyframes
    nh.param<float>("surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
    nh.param<float>("surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
    // nh.param<float>("surroundingKeyframeDensity", surroundingKeyframeDensity, 2.0);
    // nh.param<float>("surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);

    // Visualization
    nh.param<float>("globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
    nh.param<float>("globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
    nh.param<float>("globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);

    // visual ikdtree map
    nh.param<bool>("visulize_IkdtreeMap", visulize_IkdtreeMap, false);

    // reconstruct ikdtree
    nh.param<bool>("recontructKdTree", recontructKdTree, false);
}

void LoadGTSAM(ros::NodeHandle& nh) {
    LoadParams(nh);
    LoadParamsLoop(nh);

    // 自定义
    nh.param<bool>("EN_LOOP", EN_LOOP, false);
    nh.param<bool>("EN_GTSAM", EN_GTSAM, false);
    nh.param<bool>("EN_SUBMAP", EN_SUBMAP, false);

    std::cout << std::endl;
    ROS_INFO_STREAM("\033[1;32m EN_GTSAM: " << EN_GTSAM << "\033[0m");
    ROS_INFO_STREAM("\033[1;32m EN_LOOP: " << EN_LOOP << "\033[0m");
    ROS_INFO_STREAM("\033[1;32m EN_SUBMAP: " << EN_SUBMAP << "\033[0m");

    // ISAM2参数
    gtsam::ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.01;
    parameters.relinearizeSkip = 1;
    isam = new gtsam::ISAM2(parameters);

    for (int i = 0; i < 6; ++i) {
        transformTobeMapped[i] = 0;
    }
    downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);

    pubPathUpdate = nh.advertise<nav_msgs::Path>("/gtsam/path_update", 100000);
    pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/gtsam/keyframe_submap", 1);       // 发布局部关键帧map的特征点云
    pubOptimizedGlobalMap = nh.advertise<sensor_msgs::PointCloud2>("/gtsam/map_global_optimized", 1);  // 发布局部关键帧map的特征点云
}